/*
 * Copyright (c) 2021 Huawei Device Co., Ltd.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "ext_dev.h"
#include "ext_dev_if.h"
#include "pwm_if.h"

#define SG90_PERIOD 20000000        // SG90舵机20ms时钟基准
#define SG90_DUTY_MIN 500000        // SG90舵机0.5ms对应 0°,但bes2600占空比精确度为1%,0.5对应2.5%
#define SG90_DUTY_MAX 2500000       // SG90舵机2.5ms对应 180°
#define SG90_ROTATION_ANGLE_MAX 180 // SG90舵机旋转最大角度180°

static DevHandle g_sg90PwmHandle = NULL;
static struct PwmConfig g_sg90PwmConfig = {
    .duty = SG90_DUTY_MIN,
    .period = SG90_PERIOD,
    .number = 0,
    .polarity = PWM_NORMAL_POLARITY,
    .status = PWM_DISABLE_STATUS,
};

ExtDevCtlRst ExtDevSetSg90Angle(uint8_t rotationAngle)
{
    if (SG90_ROTATION_ANGLE_MAX < rotationAngle) {
        rotationAngle = SG90_ROTATION_ANGLE_MAX;
        LOG_E("Set rotationAngle out the limit, set max! ");
    }
    if (NULL == g_sg90PwmHandle) {
        LOG_E("You should init sg90 success before set it.");
        return EXT_DEV_ERROR;
    }
    PwmDisable(g_sg90PwmHandle);
    if (0 != PwmSetDuty(g_sg90PwmHandle,
                        SG90_DUTY_MIN + rotationAngle * (SG90_DUTY_MAX - SG90_DUTY_MIN) / SG90_ROTATION_ANGLE_MAX)) {
        LOG_E("Set sg90 pwm duty failed! ");
        return EXT_DEV_ERROR;
    }
    PwmEnable(g_sg90PwmHandle);

    return EXT_DEV_SUCCESS;
}

ExtDevCtlRst ExtDevSetSg90Close(void)
{
    if (NULL == g_sg90PwmHandle) {
        LOG_E("You should init sg90 success before set it.");
        return EXT_DEV_ERROR;
    }
    PwmClose(g_sg90PwmHandle);
    return EXT_DEV_SUCCESS;
}

/// 舵机初始化
ExtDevCtlRst ExtDevSg90Init(void)
{
    g_sg90PwmHandle = PwmOpen(0);
    if (NULL == g_sg90PwmHandle) {
        LOG_E("open pwm failed!");
    }
    if (0 != PwmSetConfig(g_sg90PwmHandle, &g_sg90PwmConfig)) {
        LOG_E("config pwm failed!");
    }
    return EXT_DEV_SUCCESS;
}